import Glibc

class Timer {
    private let CLOCK_REALTIME = 0
    private var start_timespec = timespec()
    private var end_timespec = timespec()
    private var time_spec = timespec()
    func start() {
        clock_gettime(Int32(CLOCK_REALTIME),&start_timespec)
    }
    
    func stop() -> Double {
        clock_gettime(Int32(CLOCK_REALTIME),&end_timespec)
        let start_time = Double(start_timespec.tv_sec * 1_000_000 + start_timespec.tv_nsec / 1_000)
        let end_time = Double(end_timespec.tv_sec * 1_000_000 + end_timespec.tv_nsec / 1_000)
        let time = end_time - start_time
        return time / 1_000
    }
    func getTime() -> Double {
        clock_gettime(Int32(CLOCK_REALTIME),&time_spec)
        return Double(time_spec.tv_sec * 1_000_000 + time_spec.tv_nsec / 1_000)
    }
}

let timer = Timer()
class Px {                               
    var V: [Double] = []
    init(x: Double, y: Double, z: Double) {
        self.V = [x, y, z, 1]
    }
}

class QType {
    var arr: [Px] = []
    var lastPx: Double = 0
    var lastX: Double = 0
    var lastY: Double = 0
    var normal: [[Double]] = []
    var line: [Bool] = []
    var edge: [[Double]] = []
    var numPx: Double = 0
    init() {}
}

func run3d() {
    let Q: QType = QType()
    var MTrans: [[Double]] = []         
    var MQube: [[Double]] = []         
    var I: [[Double]] = []
    var Origin: [Double] = []
    var Testing = [
        "loopCount": 0,
        "loopMax": 0
    ]
    // var LoopTimer;

    let validation = [
        20: 2889.0000000000045,
        40: 2889.0000000000055,
        80: 2889.000000000005,
        160: 2889.0000000000055
    ]

    func drawLine(from: Px, to: Px) {
        let x1 = from.V[0]
        let x2 = to.V[0]
        let y1 = from.V[1]
        let y2 = to.V[1]
        let dx = Int(abs(x2 - x1))     
        let dy = Int(abs(y2 - y1))
        var x = x1
        var y = y1
        var incX1, incY1: Int
        var incX2, incY2: Int
        var den: Int
        var num: Int
        var numAdd: Int
        var numPix: Int           

        if (x2 >= x1) {
            incX1 = 1
            incX2 = 1
        } else {
            incX1 = -1
            incX2 = -1
        }

        if (y2 >= y1) {
            incY1 = 1
            incY2 = 1
        } else {
            incY1 = -1
            incY2 = -1
        }

        if (dx >= dy) {
            incX1 = 0
            incY2 = 0
            den = dx
            num = dx / 2
            numAdd = Int(dy)
            numPix = dx
        } else {
            incX2 = 0
            incY1 = 0
            den = dy
            num = dy / 2
            numAdd = dx
            numPix = dy
        }

        numPix = Int(Q.lastPx) + numPix

        var i = Int(Q.lastPx)
        while i < numPix {
            num += numAdd
            if (num >= den) {
                num -= den
                x += Double(incX1)
                y += Double(incY1)
            }
            
            x += Double(incX2)
            y += Double(incY2)
            
            i += 1
        }
        Q.lastX = x;
        Q.lastY = y;
        Q.lastPx = Double(numPix)
    }

    func calcCross(V0: [Double], V1: [Double]) -> [Double] {
         var cross = [Double]()
        cross.append(V0[1]*V1[2] - V0[2]*V1[1])
        cross.append(V0[2]*V1[0] - V0[0]*V1[2])
        cross.append(V0[0]*V1[1] - V0[1]*V1[0])
        return cross
    }

    func calcNormal(V0: [Double], V1: [Double], V2: [Double]) -> [Double] {
        var A = [Double]()
        var B = [Double]()
        for i in 0..<3 {
            A.append(V0[i] - V1[i])
            B.append(V2[i] - V1[i])
        }

        A = calcCross(V0: A, V1: B)
        let length = sqrt(A[0]*A[0] + A[1]*A[1] + A[2]*A[2])
        for i in 0..<3 {
            A[i] /= length
        }
        A.append(1.0)
        return A
    }

    // multiplies two matrices
    func mMulti(M1: [[Double]], M2: [[Double]]) -> [[Double]] {
        var M = Array(repeating: Array(repeating: 0.0, count: 4), count: 4)
        
        for i in 0..<4 {
            for j in 0..<4 {
                M[i][j] = M1[i][0] * M2[0][j] + M1[i][1] * M2[1][j] + M1[i][2] * M2[2][j] + M1[i][3] * M2[3][j]
            }
        }
        
        return M
    }

    // multiplies matrix with vector
    func vMulti(M: [[Double]], V: [Double]) -> [Double] {
        var vect = [Double]()
        for i in 0..<4 {
            vect.append(M[i][0] * V[0] + M[i][1] * V[1] + M[i][2] * V[2] + M[i][3] * V[3])
        }
        return vect
    }

    func vMulti2(M: [[Double]], V: [Double]) -> [Double] {
        var vect = [Double]()
        for i in 0..<3 {
            vect.append(M[i][0] * V[0] + M[i][1] * V[1] + M[i][2] * V[2])
        }
        return vect
    }

    // add to matrices   
    func mAdd(M1: [[Double]], M2: [[Double]]) -> [[Double]] {
        var M = Array(repeating: Array(repeating: 0.0, count: 4), count: 4)
        
        for i in 0..<4 {
            for j in 0..<4 {
                M[i][j] = M1[i][j] + M2[i][j]
            }
        }
        
        return M
    }

    func translate(M: [[Double]], Dx: Double, Dy: Double, Dz: Double) -> [[Double]] {
        let T = [
            [1, 0, 0, Dx],
            [0, 1, 0, Dy],
            [0, 0, 1, Dz],
            [0, 0, 0, 1]
        ]
        
        return mMulti(M1: T, M2: M)
    }

    func rotateX(M: [[Double]], phi: Double) -> [[Double]] {
        var a = phi
        a *= Double.pi / 180.0
        
        let cosA = cos(a)
        let sinA = sin(a)
        
        let R = [
            [1, 0, 0, 0],
            [0, cosA, -sinA, 0],
            [0, sinA, cosA, 0],
            [0, 0, 0, 1]
        ]
        
        return mMulti(M1: R, M2: M)
    }

    func rotateY(M: [[Double]], phi: Double) -> [[Double]] {
        var a = phi
        a *= Double.pi / 180.0
        
        let cosA = cos(a)
        let sinA = sin(a)
        
        let R = [
            [cosA, 0, sinA, 0],
            [0, 1, 0, 0],
            [-sinA, 0, cosA, 0],
            [0, 0, 0, 1]
        ]
        
        return mMulti(M1: R, M2: M)
    }

    func rotateZ(M: [[Double]], phi: Double) -> [[Double]] {
        var a = phi
        a *= Double.pi / 180.0
        
        let cosA = cos(a)
        let sinA = sin(a)
        
        let R = [
            [cosA, -sinA, 0, 0],
            [sinA, cosA, 0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]
        ]
        
        return mMulti(M1: R, M2: M)
    }

    func drawQube() {
        var CurN = [[Double]]()
        var i = 5
        Q.lastPx = 0
        
        for _ in 0...i {
            CurN.append(vMulti2(M: MQube, V: Q.normal[i]))     
            i -= 1
        }
        
        if CurN[0][2] < 0 {
            if !Q.line[0] { drawLine(from:Q.arr[0], to:Q.arr[1]); Q.line[0] = true }
            if !Q.line[1] { drawLine(from:Q.arr[1], to:Q.arr[2]); Q.line[1] = true }
            if !Q.line[2] { drawLine(from:Q.arr[2], to:Q.arr[3]); Q.line[2] = true }
            if !Q.line[3] { drawLine(from:Q.arr[3], to:Q.arr[0]); Q.line[3] = true }
        }
        if CurN[1][2] < 0 {
            if !Q.line[2] { drawLine(from:Q.arr[3], to:Q.arr[2]); Q.line[2] = true }
            if !Q.line[9] { drawLine(from:Q.arr[2], to:Q.arr[6]); Q.line[9] = true }
            if !Q.line[6] { drawLine(from:Q.arr[6], to:Q.arr[7]); Q.line[6] = true }
            if !Q.line[10] { drawLine(from:Q.arr[7], to:Q.arr[3]); Q.line[10] = true }
        }
        if CurN[2][2] < 0 {
            if !Q.line[4] { drawLine(from:Q.arr[4], to:Q.arr[5]); Q.line[4] = true }
            if !Q.line[5] { drawLine(from:Q.arr[5], to:Q.arr[6]); Q.line[5] = true }
            if !Q.line[6] { drawLine(from:Q.arr[6], to:Q.arr[7]); Q.line[6] = true }
            if !Q.line[7] { drawLine(from:Q.arr[7], to:Q.arr[4]); Q.line[7] = true }
        }
        if CurN[3][2] < 0 {
            if !Q.line[4] { drawLine(from:Q.arr[4], to:Q.arr[5]); Q.line[4] = true }
            if !Q.line[8] { drawLine(from:Q.arr[5], to:Q.arr[1]); Q.line[8] = true }
            if !Q.line[0] { drawLine(from:Q.arr[1], to:Q.arr[0]); Q.line[0] = true }
            if !Q.line[11] { drawLine(from:Q.arr[0], to:Q.arr[4]); Q.line[11] = true }
        }
        if CurN[4][2] < 0 {
            if !Q.line[11] { drawLine(from:Q.arr[4], to:Q.arr[0]); Q.line[11] = true }
            if !Q.line[3] { drawLine(from:Q.arr[0], to:Q.arr[3]); Q.line[3] = true }
            if !Q.line[10] { drawLine(from:Q.arr[3], to:Q.arr[7]); Q.line[10] = true }
            if !Q.line[7] { drawLine(from:Q.arr[7], to:Q.arr[4]); Q.line[7] = true }
        }
        if CurN[5][2] < 0 {
            if !Q.line[8] { drawLine(from:Q.arr[1], to:Q.arr[5]); Q.line[8] = true }
            if !Q.line[5] { drawLine(from:Q.arr[5], to:Q.arr[6]); Q.line[5] = true }
            if !Q.line[9] { drawLine(from:Q.arr[6], to:Q.arr[2]); Q.line[9] = true }
            if !Q.line[1] { drawLine(from:Q.arr[2], to:Q.arr[1]); Q.line[1] = true }
        }
        
        Q.line = [false, false, false, false, false, false, false, false, false, false, false, false]
        Q.lastPx = 0
    }

    func loop() {
        if Testing["loopCount"]! > Testing["loopMax"]! {
            return
        }
        
        var testingStr = String(Testing["loopCount"]!)
        while testingStr.count < 3 {
            testingStr = "0" + testingStr
        }
        
        var MTrans = translate(M:I, Dx:-Q.arr[8].V[0], Dy:-Q.arr[8].V[1], Dz:-Q.arr[8].V[2])      
        MTrans = rotateX(M: MTrans, phi: 1)
        MTrans = rotateY(M: MTrans, phi: 3)
        MTrans = rotateZ(M: MTrans, phi: 5)
        MTrans = translate(M:MTrans, Dx:Q.arr[8].V[0], Dy:Q.arr[8].V[1], Dz:Q.arr[8].V[2])
        
        MQube = mMulti(M1: MTrans, M2: MQube)
        
        var i = 8
        while i > -1 {
            Q.arr[i].V = vMulti(M: MTrans, V: Q.arr[i].V)
            i -= 1
        }
        
        drawQube()
        Testing["loopCount"]! += 1
        loop()
    }

    func Init(cubeSize: Double) {        
        // Init/reset vars
        Origin = [150, 150, 20, 1]
        Testing["loopCount"]! = 0
        Testing["loopMax"]! = 50
        
        MTrans = [
            [1, 0, 0, 0],
            [0, 1, 0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]
        ] // transformation matrix
        
        MQube = [
            [1, 0, 0, 0],
            [0, 1, 0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]
        ] // position information of qube
        
        I = [
            [1, 0, 0, 0],
            [0, 1, 0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]
        ] // entity matrix
        
        // create qube
         Q.arr = Array(repeating:Px(x: 0, y: 0, z: 0), count:9)

        Q.arr[0] = Px(x: -cubeSize, y: -cubeSize, z: cubeSize)
        Q.arr[1] = Px(x: -cubeSize, y: cubeSize, z: cubeSize)
        Q.arr[2] = Px(x: cubeSize, y: cubeSize, z: cubeSize)
        Q.arr[3] = Px(x: cubeSize, y: -cubeSize, z: cubeSize)
        Q.arr[4] = Px(x: -cubeSize, y: -cubeSize, z: -cubeSize)
        Q.arr[5] = Px(x: -cubeSize, y: cubeSize, z: -cubeSize)
        Q.arr[6] = Px(x: cubeSize, y: cubeSize, z: -cubeSize)
        Q.arr[7] = Px(x: cubeSize, y: -cubeSize, z: -cubeSize)

        
        // // center of gravity
        Q.arr[8] = Px(x: 0, y: 0, z: 0)
        
        // anti-clockwise edge check
        Q.edge = [[0, 1, 2], [3, 2, 6], [7, 6, 5], [4, 5, 1], [4, 0, 3], [1, 5, 6]]
        
        // calculate squad normals
        Q.normal = []
        for i in 0..<Q.edge.count {
            Q.normal.append(calcNormal(V0: Q.arr[Int(Q.edge[i][0])].V, V1: Q.arr[Int(Q.edge[i][1])].V, V2: Q.arr[Int(Q.edge[i][2])].V))      
        }
        
        // // line drawn?
        Q.line = [false,false,false,false,false,false,false,false,false,false,false,false]
        
        // // create line pixels
        Q.numPx = 9 * 2 * cubeSize
        // for i in 0..<Q.numPx { createP(0,0,0) }
        
        MTrans = translate(M: MTrans, Dx: Origin[0], Dy: Origin[1], Dz: Origin[2])
        MQube = mMulti(M1: MTrans, M2: MQube)                           
        
        var i = 0
        while i < 9 {
            Q.arr[i].V = vMulti(M: MTrans, V: Q.arr[i].V)
            i += 1
        }
        
        drawQube()
        loop()
        
        // // Perform a simple sum-based verification.
        var sum:Double = 0
        for i in 0..<Q.arr.count {
            let vector = Q.arr[i].V
            for j in 0..<vector.count {
                sum += vector[j]
                // print("vector[j]: \(vector[j])")
            }
        }
        
        var expected:Double = 0
        
        if cubeSize == 20 {
            expected = validation[Int(cubeSize)] ?? 0
        } else if cubeSize == 40 {
            expected = validation[Int(cubeSize)] ?? 0
        } else if cubeSize == 80 {
            expected = validation[Int(cubeSize)] ?? 0
        } else if cubeSize == 160 {
            expected = validation[Int(cubeSize)] ?? 0
        }
        
        enum VectorSumError: Error {
            case badSum(cubeSize: Double, expected: Double, actual: Double)
        }

        if sum != expected {
            do{
                throw VectorSumError.badSum(cubeSize: cubeSize, expected: expected, actual: sum)
            }catch{
                print(error)
            }
            // throw "Error: bad vector sum for CubeSize = \(cubeSize); expected \(expected) but got \(sum)"
        }
    }
    var iter=0
    while iter <= 10 {
    var i=20
        while i <= 160 {
            Init(cubeSize: Double(i))
            i *= 2
        }
        iter=iter+1;
    }
}

func runThreeDCube() -> Int {
    timer.start()
    run3d()
    let time = timer.stop()
    print("Array Access - RunThreeDCube:\t"+String(time)+"\tms");
	return  Int(time) 
}
_ = runThreeDCube()
// print(runThreeDCube())